/*
 * psm.c
 *
 *  Created on: 2022.12.28
 *      Author: lakun
 */

#include "psm.h"
#include <string.h>

static void plug_holes(psm_t* psm)
{
	int remain = psm->wpos - psm->rpos;  // Remaining data length
	if(remain != 0)  memmove(&psm->buf[0], &psm->buf[psm->rpos], remain);
	psm->wpos             = remain;
	psm->rpos             = 0;
	psm->state            = PSM_STATE_INIT;
	psm->match_start      = 0;
	psm->body_length      = 0;
	psm->identifier_index = 0;
	psm->delimiter_index  = 0;
}

int psm_recv(psm_t* psm, const void* buf, uint32_t len)
{
	uint32_t i;
	uint32_t writeable = 0, remain = 0;
	const uint8_t* p = (const uint8_t *)buf;
	remain = psm->bufsize - psm->wpos;
	writeable = remain > len ? len : remain;
	for(i = 0; i < writeable; i++)    psm->buf[psm->wpos++] = *p++;
	return writeable;
}

int psm_recv_byte(psm_t* psm, uint8_t c)
{
	if(psm->wpos >= psm->bufsize)
		return -1;
	psm->buf[psm->wpos++] = c;
	return 0;
}

int psm_unpack(psm_t* psm)
{
    uint32_t i = 0;
    uint8_t c;

    while(psm->rpos != psm->wpos)
    {
    	c = psm->buf[psm->rpos++];

		switch((int)psm->state)
		{
		case PSM_STATE_INIT:
		{
			if(psm->identifier_bytes != 0)
			{
				if(psm->state == PSM_STATE_INIT)
				{
					if(c != psm->identifier[psm->identifier_index])
					{
						plug_holes(psm);
						break;
					}
					if(++psm->identifier_index == psm->identifier_bytes)
					{
						psm->match_start      = psm->rpos - psm->identifier_bytes;  // Recording start position
						psm->state            = PSM_STATE_BODY_BEFORE;              // Match identifier success
					}
					break;
				}
			}else
			{
				if(psm->state == PSM_STATE_INIT)
				{
					psm->match_start = psm->rpos - 1;                      // Recording start position
					psm->state       = PSM_STATE_BODY_BEFORE;              // Match identifier success
				}
			}
		}
		case PSM_STATE_BODY_BEFORE:
		{
			if(psm->rpos < (psm->body_offset + psm->match_start))  // Match before body
				break;

			if(psm->length_field_bytes != 0)  // Resolve the body data length when the length field is set。
			{
				if(psm->state == PSM_STATE_BODY_BEFORE)  //  When the body length is not resolve.
				{
					if(psm->length_field_coding == PSM_UNPACK_ENCODE_BY_BIG_ENDIAN)
					{
						for(i = psm->match_start + psm->length_field_offset; i < psm->match_start + psm->length_field_offset + psm->length_field_bytes; i++)
						{
							psm->body_length <<= 8;
							psm->body_length |= psm->buf[i];
						}
					}else
					{
						for(i = psm->length_field_offset + psm->match_start + psm->length_field_bytes - 1; i >= psm->length_field_offset; i--)
						{
							psm->body_length <<= 8;
							psm->body_length |= psm->buf[i];
						}
					}
				}
			}
			psm->state = PSM_STATE_BODY;
		}

		case PSM_STATE_BODY:
		{
			if(psm->rpos < (psm->match_start + psm->body_offset + psm->body_length))  // Match body
				break;
			if(psm->delimiter_bytes == 0)  // When the delimiter is not set, the matching has ended
			{
				psm->recv_callback(psm, &psm->buf[psm->match_start], psm->rpos - psm->match_start);
				plug_holes(psm);
				break;
			}
			psm->state = PSM_STATE_BODY_AFTER;
			break;
		}

		case PSM_STATE_BODY_AFTER:
		{
			if(c != psm->delimiter[psm->delimiter_index])
			{
				psm->delimiter_index = c == psm->delimiter[0] ? 1 : 0;
				break;
			}
			if(++psm->delimiter_index == psm->delimiter_bytes)  // Match delimiter success
			{
				psm->recv_callback(psm, &psm->buf[psm->match_start], psm->rpos - psm->match_start);
				plug_holes(psm);
			}
			break;
		}

		default:break;
		}
    }

    return 0;
}

void psm_reset(psm_t* psm)
{
	psm->wpos             = 0;
	psm->rpos             = 0;
	psm->state            = PSM_STATE_INIT;
	psm->match_start      = 0;
	psm->body_length      = 0;
	psm->identifier_index = 0;
	psm->delimiter_index  = 0;
}

int psm_unpack_custom(psm_t* psm)
{
    uint32_t i = 0;
    uint8_t c;

    while(psm->rpos != psm->wpos)
    {
    	c = psm->buf[psm->rpos++];

		switch((int)psm->state)
		{
		case PSM_STATE_INIT:
		{
			if(psm->identifier_bytes != 0)
			{
				if(psm->state == PSM_STATE_INIT)
				{
					if(c != psm->identifier[psm->identifier_index])
					{
						plug_holes(psm);
						break;
					}
					if(++psm->identifier_index == psm->identifier_bytes)
					{
						psm->match_start      = psm->rpos - psm->identifier_bytes;  // Recording start position
						psm->state            = PSM_STATE_BODY_BEFORE;              // Match identifier success
					}
					break;
				}
			}else
			{
				if(psm->state == PSM_STATE_INIT)
				{
					psm->match_start = psm->rpos - 1;                      // Recording start position
					psm->state       = PSM_STATE_BODY_BEFORE;              // Match identifier success
				}
			}
		}
		case PSM_STATE_BODY_BEFORE:
		{
			if(psm->rpos < (psm->body_offset + psm->match_start))  // Match before body
				break;

			if(psm->length_field_bytes != 0)  // Resolve the body data length when the length field is set。
			{
				if(psm->state == PSM_STATE_BODY_BEFORE)  //  When the body length is not resolve.
				{
					if(psm->length_field_coding == PSM_UNPACK_ENCODE_BY_BIG_ENDIAN)
					{
						for(i = psm->match_start + psm->length_field_offset; i < psm->match_start + psm->length_field_offset + psm->length_field_bytes; i++)
						{
							psm->body_length <<= 8;
							psm->body_length |= psm->buf[i];
						}
					}else
					{
						for(i = psm->length_field_offset + psm->match_start + psm->length_field_bytes - 1; i >= psm->length_field_offset; i--)
						{
							psm->body_length <<= 8;
							psm->body_length |= psm->buf[i];
						}
					}
					psm->body_length += 2;
				}
			}
			psm->state = PSM_STATE_BODY;
		}

		case PSM_STATE_BODY:
		{
			if(psm->rpos < (psm->match_start + psm->body_offset + psm->body_length))  // Match body
				break;
			if(psm->delimiter_bytes == 0)  // When the delimiter is not set, the matching has ended
			{
				psm->recv_callback(psm, &psm->buf[psm->match_start], psm->rpos - psm->match_start);
				plug_holes(psm);
				break;
			}
			psm->state = PSM_STATE_BODY_AFTER;
			break;
		}

		case PSM_STATE_BODY_AFTER:
		{
			if(c != psm->delimiter[psm->delimiter_index])
			{
				psm->delimiter_index = c == psm->delimiter[0] ? 1 : 0;
				break;
			}
			if(++psm->delimiter_index == psm->delimiter_bytes)  // Match delimiter success
			{
				psm->recv_callback(psm, &psm->buf[psm->match_start], psm->rpos - psm->match_start);
				plug_holes(psm);
			}
			break;
		}

		default:break;
		}
    }

    return 0;
}

